﻿namespace Pathfinding
{
    using System;
    using System.Collections.Generic;
    using System.Linq;

    public class Pathfinder
    {
        internal HashSet<Point3D> dynamicObstacles = new HashSet<Point3D>();
        private static Surr[] surrounding = new Surr[] { new Surr(0, 1, 0), new Surr(0, 0, 1), new Surr(-1, 0, 0), new Surr(1, 0, 0), new Surr(0, 0, -1), new Surr(0, -1, 0) };
        private HashSet<Point3D> walkable = new HashSet<Point3D>();

        public SearchNode FindPath(Point3D start, Point3D end)
        {
            return this.FindPathReversed(end, start);
        }

        private SearchNode FindPathReversed(Point3D start, Point3D end)
        {
            SearchNode item = new SearchNode(start, 0, 0, null);
            if (this == null)
            {
                return item;
            }
            MinHeap heap = new MinHeap();
            heap.Add(item);
            HashSet<Point3D> set = new HashSet<Point3D> {
                start
            };
            while (heap.HasNext())
            {
                SearchNode next = heap.ExtractFirst();
                if (next.position.GetDistanceSquared(end) <= 1)
                {
                    return new SearchNode(end, next.pathCost + 1, next.cost + 1, next);
                }
                for (int i = 0; i < surrounding.Length; i++)
                {
                    Surr surr = surrounding[i];
                    Point3D position = new Point3D(next.position, surr.Point);
                    bool flag = this.IsPositionFree(position);
                    if (this.dynamicObstacles.Contains(position))
                    {
                        flag = false;
                    }
                    if (position == start)
                    {
                        flag = true;
                    }
                    if (position == end)
                    {
                        flag = true;
                    }
                    if ((flag && !set.Contains(position)) && (next.position.GetDistanceSquared(position) <= 1))
                    {
                        set.Add(position);
                        int pathCost = next.pathCost + surr.Cost;
                        int cost = pathCost + position.GetDistanceSquared(end);
                        SearchNode node3 = new SearchNode(position, cost, pathCost, next);
                        heap.Add(node3);
                    }
                }
            }
            return null;
        }

        public Point3D[] GetAllWalkable()
        {
            return (from x in this.walkable
                where !this.dynamicObstacles.Contains(x)
                select x).ToArray<Point3D>();
        }

        public bool IsPositionFree(Point3D position)
        {
            return this.walkable.Contains(position);
        }

        public void MarkBlocked(Point3D position)
        {
            this.walkable.Remove(position);
        }

        public void MarkWalkable(Point3D position)
        {
            this.walkable.Add(position);
        }

        private class Surr
        {
            public int Cost;
            public Point3D Point;

            public Surr(int x, int y, int z)
            {
                this.Point = new Point3D(x, y, z);
                this.Cost = ((x * x) + (y * y)) + (z * z);
            }
        }
    }
}

